#include "gimbal.h"
#include "robot_def.h"

#include "general_def.h"
#include "message_center.h"
#include "mpu6500.h"
#include "dji_motor.h"
#include "usart.h"

//引入自定义控制器发送模块头文件
#include "customer_controller.h"

//引入SMR战队的CRC校验算法
#include "CRC8_CRC16.h"


/***********函数定义区******************/

static void Data_Concatenation(uint8_t *data, uint16_t data_lenth);

/*************全局变量区*****************/

static Controller_t tx_data; // 自定义控制器发送的数据

static Customer_Controller_Calibration_s calibration_data;


static Publisher_t *gimbal_pub; 
static Subscriber_t *gimbal_sub;
//yaw 10-4 mid 7.4 pitch mid 7  
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息

static CC_ctrl_t *customer_controller_send_data;

static DJIMotorInstance *joint1_3508, *joint4_3508,*joint5_2006,*joint6_3508;//自定义控制器上的大疆电机
//还需要电位器的ADC，用来构建自定义控制器的第2、3轴

//新建一组变量来存放校准完成后的零点，这些变量的值不是0，而是对应一个角度值。这个角度值是刚上电时读取到的total_angle
static float joint1_3508_zero;
static float joint4_3508_zero;
static float joint5_2006_zero;
static float joint6_3508_zero;

//新建一组变量来存放校准完成后要发给机械臂的角度，使用实时读取到的total_angle减去校准完成后的零点，就可以算出真实角度
static float joint1_real_angle;
static float joint4_real_angle;
static float joint5_real_angle;
static float joint6_real_angle;

//校准误差，单位为度，用来判断零点的角度是否被写入jointx_zero
static float calibration_error = 2.0f;

#define GIMBAL_PITCH_MAX_Duty 30
#define GIMBAL_PITCH_MIN_Duty -30
#define GIMBAL_YAW_MAX_Duty 30
#define GIMBAL_YAW_MIN_Duty -30 

//SMR战队的自定义控制器发送函数
/**
 * @brief 数据拼接函数，将帧头、命令码、数据段、帧尾头拼接成一个数组
 * @param data 数据段的数组指针
 * @param data_lenth 数据段长度
 */
static void Data_Concatenation(uint8_t *data, uint16_t data_lenth)
{
    static uint8_t seq = 0;
    /// 帧头数据
    tx_data.frame_header.sof = 0xA5;                              // 数据帧起始字节，固定值为 0xA5
    tx_data.frame_header.data_length = data_lenth;                // 数据帧中数据段的长度
    tx_data.frame_header.seq = seq++;                             // 包序号
    append_CRC8_check_sum((uint8_t *)(&tx_data.frame_header), 5); // 添加帧头 CRC8 校验位
    /// 命令码ID
    tx_data.cmd_id = CONTROLLER_CMD_ID;
    /// 数据段
    memcpy(tx_data.data, data, data_lenth);
    /// 帧尾CRC16，整包校验
    append_CRC16_check_sum((uint8_t *)(&tx_data), DATA_FRAME_LENGTH);
}

void GimbalInit()
{
    // 所有3508的参数一样,改tx_id和反转标志位、电机类型即可

    //等测试完通信功能后考虑把电机加上电流力矩保持在原位，或者用位置锁存

    Motor_Init_Config_s chassis_motor_config = {
        .can_init_config.can_handle = &hcan1,//全部挂在can1上
        .controller_param_init_config = {//只是把电机当做角度传感器，所有的PID都不要了，电机无须发力
            .speed_PID = {
                .Kp = 0, 
                .Ki = 0,
                .Kd = 0,
                .IntegralLimit = 0,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 0,
            },
            .current_PID = {
                .Kp = 0,
                .Ki = 0,
                .Kd = 0,
                .IntegralLimit = 0,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 0,
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
        },
        .motor_type = M3508,
    };
    //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
    chassis_motor_config.can_init_config.tx_id = 1;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    joint1_3508 = DJIMotorInit(&chassis_motor_config);

    chassis_motor_config.can_init_config.tx_id = 2;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    joint4_3508 = DJIMotorInit(&chassis_motor_config);

    chassis_motor_config.can_init_config.tx_id = 3;
    chassis_motor_config.motor_type = M2006;//这里改为M2006
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    joint5_2006 = DJIMotorInit(&chassis_motor_config);

    chassis_motor_config.can_init_config.tx_id = 4;
    chassis_motor_config.motor_type = M3508;//这里又改回M3508
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    joint6_3508 = DJIMotorInit(&chassis_motor_config);

    //使用A板中间的那个串口进行数据发送
    customer_controller_send_data = Customer_Controller_Init(&huart8);



    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
}

/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask()
{
    // 获取云台控制数据
    // 后续增加未收到数据的处理
    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);

    uint8_t data[DATA_LENGTH] = {0x40, 0x50, 0x60, 0x70};


    //上电校准，把当前的单圈角度设置为0点，到时直接把这个角度值发给机械臂
    //校准参考机械臂的校准，机械臂joint1校准完成时6020的单圈角度是142ecd对应的角度，自定义控制器的校准完成角度也应该为这个值
    //joint2校准完成后起始角度是0，joint2需要弧度制的单位，joint2的活动范围是0到-3.75
    //joint3校准完成后起始角度是0，joint2需要弧度制的单位，joint2的活动范围是0到2.55
    //joint4校准完成后的起始角度是2039对应的角度值
    //joint5校准完成后的起始值是机械臂自己校准完成后的中值
    //joint6校准完成后的起始值是机械臂自己校准完成后的中值
    
    // //如果校准未完成，开始校准
    // if (!calibration_data.calibration_OK)
    // {
    //     //把零点设置成上电时读到的角度
    //     joint1_3508_zero = joint1_3508->measure.total_angle;
    //     joint4_3508_zero = joint4_3508->measure.total_angle;
    //     joint5_2006_zero = joint5_2006->measure.total_angle;
    //     joint6_3508_zero = joint6_3508->measure.total_angle;
        
    //     //校准完成，将标志位置为真，不再进行校准
    //     if ((joint1_3508->measure.total_angle - joint1_3508_zero)<calibration_error&&
    //         (joint4_3508->measure.total_angle - joint4_3508_zero)<calibration_error&&
    //         (joint5_2006->measure.total_angle - joint5_2006_zero)<calibration_error&&
    //         (joint6_3508->measure.total_angle - joint6_3508_zero)<calibration_error)
    //     {
    //         calibration_data.calibration_OK = true ;
    //     }
        
        
    // }

        //如果校准未完成，开始校准
    if (!calibration_data.calibration_OK)
    {
        // 检查所有 total_angle 是否都已经更新为有效值
        if (joint1_3508->measure.total_angle != 0 &&
            joint4_3508->measure.total_angle != 0 &&
            joint5_2006->measure.total_angle != 0 &&
            joint6_3508->measure.total_angle != 0)
        {
            //把零点设置成上电时读到的角度
            joint1_3508_zero = joint1_3508->measure.total_angle;
            joint4_3508_zero = joint4_3508->measure.total_angle;
            joint5_2006_zero = joint5_2006->measure.total_angle;
            joint6_3508_zero = joint6_3508->measure.total_angle;

            //校准完成，将标志位置为真，不再进行校准
            if ((joint1_3508->measure.total_angle - joint1_3508_zero)<calibration_error&&
                (joint4_3508->measure.total_angle - joint4_3508_zero)<calibration_error&&
                (joint5_2006->measure.total_angle - joint5_2006_zero)<calibration_error&&
                (joint6_3508->measure.total_angle - joint6_3508_zero)<calibration_error)
            {
                calibration_data.calibration_OK = true ;
            }
        }
    }

    //计算出真正的角度
    //真正的角度等于实时读到的角度减去零点对应的角度
    joint1_real_angle = joint1_3508->measure.total_angle - joint1_3508_zero;
    joint4_real_angle = joint4_3508->measure.total_angle - joint4_3508_zero;
    joint5_real_angle = joint5_2006->measure.total_angle - joint5_2006_zero;
    joint6_real_angle = joint6_3508->measure.total_angle - joint6_3508_zero;

    //对真正的角度进行窗口滤波
    


    // customer_controller_send_data->joint_1_angle = joint1_3508->measure.total_angle;

    // //发送出去的第二轴角度为电位器ADC值
    // //customer_controller_send_data->joint_2_angle =

    // //发送出去的第三轴角度为电位器ADC值
    // //customer_controller_send_data->joint_3_angle = 

    // customer_controller_send_data->joint_4_angle = joint4_3508->measure.total_angle;

    // customer_controller_send_data->joint_5_angle = joint5_2006->measure.total_angle;

    // customer_controller_send_data->joint_6_angle = joint6_3508->measure.total_angle;


    //拼接数据段
    Data_Concatenation(data, DATA_LENGTH);

    //使用实例化的串口发送
    // USARTSend(&huart8,(uint8_t *)(&tx_data),sizeof(tx_data),USART_TRANSFER_DMA);

    Customer_Controller_Send((uint8_t *)(&tx_data));


    // 推送消息
    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
   
}